﻿#include "meTransform.h"
#include "memath.h"

#include <Eigen/Dense>

Matrix4d MeTransform::rotateX(const double degree)
{
    Matrix4d m;
    rotateX(degree, &m);

    return m;
}

void MeTransform::rotateX(const double degree, Matrix4d *m)
{
    m->put(0, 0, 1);
    m->put(1, 0, 0);
    m->put(2, 0, 0);
    m->put(3, 0, 0);

    m->put(0, 1, 0);
    m->put(1, 1, cos(degree));
    m->put(2, 1, sin(degree));
    m->put(3, 1, 0);

    m->put(0, 2, 0);
    m->put(1, 2, -sin(degree));
    m->put(2, 2, cos(degree));
    m->put(3, 2, 0);

    m->put(0, 3, 0);
    m->put(1, 3, 0);
    m->put(2, 3, 0);
    m->put(3, 3, 1);

//    MatrixUtil::convertTinyToZero(*m);
}

Matrix4d MeTransform::rotateY(const double degree)
{
    Matrix4d m;
    rotateY(degree, &m);
    return m;
}

void MeTransform::rotateY(const double degree, Matrix4d *m)
{
    m->put(0, 0, cos(degree));
    m->put(1, 0, 0);
    m->put(2, 0, -sin(degree));
    m->put(3, 0, 0);

    m->put(0, 1, 0);
    m->put(1, 1, 1);
    m->put(2, 1, 0);
    m->put(3, 1, 0);

    m->put(0, 2, sin(degree));
    m->put(1, 2, 0);
    m->put(2, 2, cos(degree));
    m->put(3, 2, 0);

    m->put(0, 3, 0);
    m->put(1, 3, 0);
    m->put(2, 3, 0);
    m->put(3, 3, 1);

//    MatrixUtil::convertTinyToZero(*m);
}

Matrix4d MeTransform::rotateZ(const double degree)
{
    Matrix4d m;
    rotateZ(degree, &m);
    return m;
}

void MeTransform::rotateZ(const double degree, Matrix4d *m)
{
    m->put(0, 0, cos(degree));
    m->put(1, 0, sin(degree));
    m->put(2, 0, 0);
    m->put(3, 0, 0);

    m->put(0, 1, -sin(degree));
    m->put(1, 1, cos(degree));
    m->put(2, 1, 0);
    m->put(3, 1, 0);

    m->put(0, 2, 0);
    m->put(1, 2, 0);
    m->put(2, 2, 1);
    m->put(3, 2, 0);

    m->put(0, 3, 0);
    m->put(1, 3, 0);
    m->put(2, 3, 0);
    m->put(3, 3, 1);

    //    MatrixUtil::convertTinyToZero(m);
}

void MeTransform::invertRotMatrix(Matrixd &mIn, Matrixd &mOut)
{
    /**
    [
        nx ox ax px
        ny oy ay py
        nz oz az pz
        0  0  0  1
    ]
    to (p = (px, py, pz), n = (nx, ny, nz), ... )
    [
        nx ny nz -p.n
        ox oy oz -p.o
        ax ay az -p.a
        0  0  0  1
    ]
          */
    // the inverse of the homogeneous transformation matrix
    // first row
    mOut.put(0, 0, mIn.get(0, 0));
    mOut.put(0, 1, mIn.get(1, 0));
    mOut.put(0, 2, mIn.get(2, 0));
    mOut.put(0, 3, -mIn.get(0, 0)*mIn.get(0, 3) - mIn.get(1,0)*mIn.get(1,3) - mIn.get(2,0)*mIn.get(2,3));


    // second row
    mOut.put(1, 0, mIn.get(0, 1));
    mOut.put(1, 1, mIn.get(1, 1));
    mOut.put(1, 2, mIn.get(2, 1));
    mOut.put(1, 3, -mIn.get(0, 1)*mIn.get(0, 3) - mIn.get(1,1)*mIn.get(1,3) - mIn.get(2,1)*mIn.get(2,3));

    // third row
    mOut.put(2, 0, mIn.get(0, 2));
    mOut.put(2, 1, mIn.get(1, 2));
    mOut.put(2, 2, mIn.get(2, 2));
    mOut.put(2, 3, -mIn.get(0, 2)*mIn.get(0, 3) - mIn.get(1,2)*mIn.get(1,3) - mIn.get(2,2)*mIn.get(2,3));

    // forth row
    mOut.put(3, 0, 0.0);
    mOut.put(3, 1, 0.0);
    mOut.put(3, 2, 0.0);
    mOut.put(3, 3, 1.0);
}

void MeTransform::eulerAngleZYZToMatrix(
        const double x, const double y, const double z,
        const double u, const double v, const double w,
        Matrixd *m)
{
    // pos to homogeneous transformation matrix

    // first row
    m->put(0, 0, cos(u)*cos(v)*cos(w)-sin(u)*sin(w));
    m->put(0, 1, -cos(u)*cos(v)*sin(w)-sin(u)*cos(w));
    m->put(0, 2, cos(u)*sin(v));
    m->put(0, 3, x);

    // second row
    m->put(1, 0, sin(u)*cos(v)*cos(w)+cos(u)*sin(w));
    m->put(1, 1, -sin(u)*cos(v)*sin(w)+cos(u)*cos(w));
    m->put(1, 2, sin(u)*sin(v));
    m->put(1, 3, y);

    // third row
    m->put(2, 0, -sin(v)*cos(w));
    m->put(2, 1, sin(v)*sin(w));
    m->put(2, 2, cos(v));
    m->put(2, 3, z);

    // forth row
    m->put(3, 0, 0.0);
    m->put(3, 1, 0.0);
    m->put(3, 2, 0.0);
    m->put(3, 3, 1.0);
}

void MeTransform::eulerAngleZYZToMatrix2(
        const double x, const double y, const double z,
        const double u, const double v, const double w, Matrixd *m)
{
    // 旋转矩阵计算
    Matrix4d mRz;
    Matrix4d mRy;
    Matrix4d mRz2;
    rotateZ(u, &mRz);
    rotateY(v, &mRy);
    rotateZ(w, &mRz2);

    *m = mRz * mRy * mRz2;

    m->put(0, 3, x);
    m->put(1, 3, y);
    m->put(2, 3, z);
}

void MeTransform::eulerAngleZYZToMatrix(double xyzuvw[], Matrixd *m)
{
    eulerAngleZYZToMatrix(xyzuvw[0],xyzuvw[1],xyzuvw[2],xyzuvw[3],xyzuvw[4],xyzuvw[5], m);
}

void MeTransform::eulerAngleZYZToMatrix(double xyz[], double uvw[], Matrixd *m)
{
    eulerAngleZYZToMatrix(xyz[0],xyz[1],xyz[2],uvw[0],uvw[1],uvw[2], m);
}

void MeTransform::eulerAngleZYXToMatrix(const double x, const double y, const double z,
            const double u, const double v, const double w, Matrixd *m)
{
    // pos to homogeneous transformation matrix
    /**
     u, v, w 绕 z, y, x
    [
        cu * cv     cu*sv*sw - su*cw   cu*sv*cw + su*sw
        su * cv     su*sv*sw + cu*cw   su*sv*cw - cu*sw
        -sv         cv * sw            cv * cw
    ]
     */
    // first row
    m->put(0, 0, cos(u)*cos(v));
    m->put(0, 1, cos(u)*sin(v)*sin(w)-sin(u)*cos(w));
    m->put(0, 2, cos(u)*sin(v)*cos(w)+sin(u)*sin(w));
    m->put(0, 3, x);

    // second row
    m->put(1, 0, sin(u)*cos(v));
    m->put(1, 1, sin(u)*sin(v)*sin(w)+cos(u)*cos(w));
    m->put(1, 2, sin(u)*sin(v)*cos(w)-cos(u)*sin(w));
    m->put(1, 3, y);

    // third row
    m->put(2, 0, -sin(v));
    m->put(2, 1, cos(v)*sin(w));
    m->put(2, 2, cos(v)*cos(w));
    m->put(2, 3, z);

    // forth row
    m->put(3, 0, 0.0);
    m->put(3, 1, 0.0);
    m->put(3, 2, 0.0);
    m->put(3, 3, 1.0);
}

void MeTransform::eulerAngleZYXToMatrix2(
        const double x, const double y, const double z,
        const double u, const double v, const double w, Matrixd *m)
{
    // 旋转矩阵计算
    Matrix4d mRz;
    Matrix4d mRy;
    Matrix4d mRx;
    rotateZ(u, &mRz);
    rotateY(v, &mRy);
    rotateX(w, &mRx);

    *m = mRz * mRy * mRx;

    m->put(0, 3, x);
    m->put(1, 3, y);
    m->put(2, 3, z);
}

void MeTransform::matrixToEulerAngleZYZ(
        const Matrixd &m, double &x, double &y, double &z, double &u, double &v, double &w)
{
    // pos from homogeneous transformation matrix
    x = m.get(0, 3);
    y = m.get(1, 3);
    z = m.get(2, 3);

    // r31*r31 + r32*r32 不能为0
    double sy = sqrt(m.get(2,0) * m.get(2,0) +  m.get(2,1) * m.get(2,1) );

    v = atan2(sy, m.get(2,2));
    if ( ME_IS_ZERO(v) ) {  // v == 0
        u = 0;
        w = atan2(-m.get(0, 1), m.get(0, 0)); // atan2(-r12,r11)
    } else if (ME_IS_ZERO(v-ME_PI)) { // v == 180
        u = 0;
        w = atan2(m.get(0, 1), -m.get(0, 0)); // atan2(r12,-r11)
    } else {
        u = atan2(m.get(1,2)/sin(v),m.get(0,2)/sin(v));
        w = atan2(m.get(2,1)/sin(v),-m.get(2,0)/sin(v));
    }
}

void MeTransform::matrixToEulerAngleZYX(const Matrixd &m, double &x, double &y, double &z,
            double &u, double &v, double &w, const double eps)
{
    /**
     u, v, w 绕 z, y, x
    [
        cu * cv     cu*sv*sw - su*cw   cu*sv*cw - su*sw
        su * cv     su*sv*sw + cu*cw   su*sv*cw - cu*sw
        -sv         cv * sw            cv * cw
    ]
    or (cv=0 ( v=90 or v = -90))
    [
        0     cu*sv*sw - su*cw   cu*sv*cw - su*sw
        0     su*sv*sw + cu*cw   su*sv*cw - cu*sw
        -sv         0            0
    ]
     */
    // pos from homogeneous transformation matrix
    x = m.get(0, 3);
    y = m.get(1, 3);
    z = m.get(2, 3);

    // r11*r11 + r21*r21 不能为0
    double sy = sqrt(m.get(0,0) * m.get(0,0) +  m.get(1,0) * m.get(1,0) );

    bool singular = sy < eps;

    if (singular) {
        // 当cv = 0, sv = 1/-1时候
        w = atan2(-m.get(1,2), m.get(1,1));
        v = atan2(-m.get(2,0), sy);
        u = 0;
    } else {
        // 如果 -90 < v < 90, 也就是 cos(v) > 0
        v = atan2(-m.get(2,0), sy);  // sy为正， 计算结果在 -pi/2， pi/2之间
        u = atan2(m.get(1,0), m.get(0,0));
        w = atan2(m.get(2, 1), m.get(2,2));  // 当 y 旋转角度不为90的时候
    }
}

void MeTransform::matrixToEulerAngleZYX(
        const Matrixd &m, double &x, double &y, double &z,
        double uvw1[], double uvw2[])
{
    /**
     u, v, w 绕 z, y, x
    [
        cu * cv     cu*sv*sw - su*cw   cu*sv*cw - su*sw
        su * cv     su*sv*sw + cu*cw   su*sv*cw - cu*sw
        -sv         cv * sw            cv * cw
    ]
     */
    // pos from homogeneous transformation matrix
    x = m.get(0, 3);
    y = m.get(1, 3);
    z = m.get(2, 3);

    // r11*r11 + r21*r21 不能为0
    double sy = sqrt(m.get(0,0) * m.get(0,0) +  m.get(1,0) * m.get(1,0) );

    bool singular = sy < 1e-6;

    if (singular) {
        // 当cv = 0, sv = 1/-1时候
        uvw1[0] = atan2(-m.get(1,2), m.get(1,1));
        uvw1[1] = atan2(-m.get(2,0), sy);
        uvw1[2] = 0;
    } else {
        // 如果 -90 < v < 90, 也就是 cos(v) > 0
        uvw1[0] = atan2(-m.get(2,0), sy);  // sy为正， 计算结果在 -pi/2， pi/2之间
        uvw1[1] = atan2(m.get(1,0), m.get(0,0));
        uvw1[2] = atan2(m.get(2, 1), m.get(2,2));  // 当 y 旋转角度不为90的时候

        // 如果 [-180, -90), (90, 180] 也就是 cos(v) < 0
        uvw2[0] = atan2(-m.get(2,0), -sy);
        uvw2[1] = atan2(-m.get(1,0), -m.get(0,0));
        uvw2[2] = atan2(-m.get(2,1), -m.get(2,2));
    }
}

Matrix4d MeTransform::translate(double x, double y, double z)
{
    Matrix4d matrix;
    matrix.identity();

    matrix[0][3] = x;
    matrix[1][3] = y;
    matrix[2][3] = z;

    return matrix;
}

Matrix4d MeTransform::rotate(double angle, double x, double y, double z)
{
    Matrix4d matrix;
    matrix.identity();

    MeTransform::rotate(angle, x, y, z, matrix);

    return matrix;
}

void MeTransform::rotate(double angle, double v[3], Matrix4d &m)
{
    MeTransform::rotate(angle, v[0], v[1], v[2], m);
}

void MeTransform::rotate(double angle, double x, double y, double z, Matrix4d &m)
{
    if (angle == 0.0 || (x == 0.0 && y == 0.0 && z == 0.0))
    {
        return;
    }
    // make a normalized quaternion
    double w = cos(0.5*angle);
    double f = sin(0.5*angle)/sqrt(x*x+y*y+z*z);
    x *= f;
    y *= f;
    z *= f;

    // convert the quaternion to a matrix
    double ww = w*w;
    double wx = w*x;
    double wy = w*y;
    double wz = w*z;

    double xx = x*x;
    double yy = y*y;
    double zz = z*z;

    double xy = x*y;
    double xz = x*z;
    double yz = y*z;

    double s = ww - xx - yy - zz;

    m[0][0] = xx*2 + s;
    m[1][0] = (xy + wz)*2;
    m[2][0] = (xz - wy)*2;
    m[3][0] = 0;

    m[0][1] = (xy - wz)*2;
    m[1][1] = yy*2 + s;
    m[2][1] = (yz + wx)*2;
    m[3][1] = 0;

    m[0][2] = (xz + wy)*2;
    m[1][2] = (yz - wx)*2;
    m[2][2] = zz*2 + s;
    m[3][2] = 0;

    m[0][3] = 0;
    m[1][3] = 0;
    m[2][3] = 0;
    m[3][3] = 1;
}

Matrix4d MeTransform::scale(double x, double y, double z)
{
    Matrix4d matrix;
    matrix.identity();
    if (x == 1.0 && y == 1.0 && z == 1.0)
    {
      return matrix;
    }

    matrix[0][0] = x;
    matrix[1][1] = y;
    matrix[2][2] = z;

    return matrix;
}

void MeTransform::rotateBetweenVectors(double v1[], double v2[], Matrix4d &m)
{
    double axis[3];
    MeMath::cross(v1, v2, axis);
    double norm = MeMath::normalize(axis);
    double angle = atan2(norm, MeMath::dot(v1, v2));

    MeTransform::rotate(angle, axis, m);
}

void MeTransform::rotationVector(double v1[], double v2[], double rotVec[])
{
    double axis[3];
    MeMath::cross(v1, v2, axis);
    double norm = MeMath::normalize(axis);
    double angle = atan2(norm, MeMath::dot(v1, v2));

    rotVec[0] = axis[0] * angle;
    rotVec[1] = axis[1] * angle;
    rotVec[2] = axis[2] * angle;
}

bool MeTransform::intersectionLinePlane(double P1[], double P2[], double n[], double point[], double outP[])
{
    double coeffi[4] = {n[0], n[1], n[2], -n[0] * point[0] - n[1] * point[1] - n[2] * point[2]};
    double P1P2[3] = {P2[0]-P1[0],P2[1]-P1[1],P2[2]-P1[2]};
    int i = 0;
    double num, den, t;
    num = coeffi[0] * P1[0] + coeffi[1] * P1[1] + coeffi[2] * P1[2] + coeffi[3];
    den = coeffi[0] * P1P2[0] + coeffi[1] * P1P2[1] + coeffi[2] * P1P2[2];
    if(fabs(den)<1e-5)
    {
        //parallel
        return false;
    }
    t=num/den;
    for(i=0;i<3;i++) {
        outP[i]=P1[i]+t*P1P2[i];
    }

    return true;
}


void MeTransform::axisesToRotationVector(double axis1[], double axis2[], double outRotVec[])
{
    double axis[3];
    MeMath::cross(axis1, axis2, axis);
    double norm = MeMath::normalize(axis);
    double theta = atan2(norm, MeMath::dot(axis1, axis2));

    outRotVec[0] =  theta*axis[0];
    outRotVec[1] =  theta*axis[1];
    outRotVec[0] =  theta*axis[2];
}

void MeTransform::axisesToRotationVector(double axis1[], double axis2[], double &angle, double outRotAxis[])
{
    double *axis = outRotAxis;
    MeMath::cross(axis1, axis2, axis);
    double norm = MeMath::normalize(axis);
    angle = atan2(norm, MeMath::dot(axis1, axis2));
}

void MeTransform::axisesToRotationMatrix(double axis1[], double axis2[], Eigen::Matrix3d &outM, double *angle, double outRotAxis[])
{
    double axis[3];
    MeMath::cross(axis1, axis2, axis);
    double norm = MeMath::normalize(axis);
    double _angle = atan2(norm, MeMath::dot(axis1, axis2));

    outM = Eigen::AngleAxisd(_angle, Eigen::Vector3d(axis)).toRotationMatrix();

    if (angle) {
        *angle = _angle;
    }
    if (outRotAxis) {
        outRotAxis[0] = axis[0];
        outRotAxis[1] = axis[1];
        outRotAxis[2] = axis[2];
    }
}

void MeTransform::rotationVectorToMatrix(const double x, const double y, const double z, const double u, const double v, const double w, Matrix4d *m)
{
    Eigen::Vector3d eiVec(u, v, w);
    double theta = eiVec.norm();
    double itheta = (theta > 1e-6) ? 1./theta : 0.;
    eiVec *= itheta;
    Eigen::AngleAxisd rotation_vector ( theta, eiVec );

    Eigen::Matrix3d rotM = rotation_vector.toRotationMatrix();

    (*m)(0,0) = rotM(0,0);
    (*m)(1,0) = rotM(1,0);
    (*m)(2,0) = rotM(2,0);
    (*m)(3,0) = 0.0;

    (*m)(0,1) = rotM(0,1);
    (*m)(1,1) = rotM(1,1);
    (*m)(2,1) = rotM(2,1);
    (*m)(3,1) = 0.0;

    (*m)(0,2) = rotM(0,2);
    (*m)(1,2) = rotM(1,2);
    (*m)(2,2) = rotM(2,2);
    (*m)(3,2) = 0.0;


    m->put(0, 3, x);
    m->put(1, 3, y);
    m->put(2, 3, z);
    m->put(3, 3, 1.0);
}

void MeTransform::rotationVectorToMatrix(double pose[6], Matrix4d *m)
{
    rotationVectorToMatrix(pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], m);
}

Matrix4d MeTransform::rigidTransform3D_old(double sp1[], double sp2[], double sp3[], double tp1[], double tp2[], double tp3[])
{
    double axisA[3],axisB[3];

    // 1. 计算平面法向量
    double sN[3]; double tN[3];
    MeMath::computeNormal(sp1, sp2, sp3, sN);
    MeMath::computeNormal(tp1, tp2, tp3, tN);

    // 2. 计算旋转轴和角度， 从sN -> tN. 这样旋转以后法向量相同
    MeMath::cross(sN, tN, axisA);
    MeMath::normalize(axisA);
    double angleA = MeMath::angleBetweenVectors(sN, tN);
    // rotate matrix
    Matrix4d matrixRotA = MeTransform::rotate(angleA, axisA[0], axisA[1], axisA[2]);
    double mp2[3], mp1[3];
    matrixRotA.multiplyPoint3D(sp2, mp2);
    matrixRotA.multiplyPoint3D(sp1, mp1);

    // 3. 绕当前的法向量 tN 旋转，让 p1->p2 方向相同
    double sv12[3], tv12[3];
    MeMath::subtract(mp2, mp1, sv12);
    MeMath::subtract(tp2, tp1, tv12);

    MeMath::cross(sv12, tv12, axisB);
    MeMath::normalize(axisB);
    double angleB = MeMath::angleBetweenVectors(sv12, tv12);
    Matrix4d matrixRotB = MeTransform::rotate(angleB, axisB[0], axisB[1], axisB[2]);

    // 4. 平移，使得 p1重合
    matrixRotB.multiplyPoint3D(mp1, mp1);
    Matrix4d matrixTrans = MeTransform::translate(tp1[0]-mp1[0], tp1[1]-mp1[1], tp1[2]-mp1[2]);

    return (matrixTrans * matrixRotB * matrixRotA);
}

bool MeTransform::rigidTransform3D(std::vector<MePoint3D> &A, std::vector<MePoint3D> &B, Matrix4d &m_AtoB)
{
    assert(A.size() == B.size());

    return rigidTransform3D(A.data(), B.data(), (uint)A.size(), m_AtoB);
}

bool MeTransform::rigidTransform3D(MePoint3D A[], MePoint3D B[], uint size, Matrix4d &m_AtoB)
{
    if (size < 3) {
        std::cout << "The size of A must be greater/equal 3." << std::endl;
        return false;
    }

    // find mean column wise
    MePoint3D centroid_A;
    MePoint3D centroid_B;
    centroid_A.zero(); centroid_B.zero();
    for (unsigned int i=0; i< size; i++) {
        centroid_A += A[i];
        centroid_B += B[i];
    }
    centroid_A /= size;
    centroid_B /= size;

    // subtract mean
    Eigen::MatrixXd Am(3, size);
    Eigen::MatrixXd Bm(3, size);
    for (unsigned int i=0; i< size; i++) {
        Am(0, i) = A[i](0) - centroid_A(0);
        Am(1, i) = A[i](1) - centroid_A(1);
        Am(2, i) = A[i](2) - centroid_A(2);

        Bm(0, i) = B[i](0) - centroid_B(0);
        Bm(1, i) = B[i](1) - centroid_B(1);
        Bm(2, i) = B[i](2) - centroid_B(2);
    }

    // calculate covariance matrix
    Eigen::MatrixXd H = Am * Bm.transpose();

    // std::cout << "H= \n" << H <<std:: endl;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::MatrixXd V = svd.matrixV(), U = svd.matrixU();

    Eigen::MatrixXd R = V*U.transpose();

    // special reflection case
    if (R.determinant() < 0) {
        //std::cout << "det(R) < R, reflection detected!, correcting for it ..." << std::endl;
        V(0, 2) *= -1;
        V(1, 2) *= -1;
        V(2, 2) *= -1;
        R = V*U.transpose();
    }


    m_AtoB.identity();
    for (unsigned i=0; i<3;i++) {
        for (unsigned j=0; j<3;j++) {
            m_AtoB(i,j) = R(i,j);
        }
    }

    MePoint3D t;
    m_AtoB.multiplyPoint3D(centroid_A, t);
    t =  centroid_B - t;
    m_AtoB(0, 3) = t(0);
    m_AtoB(1, 3) = t(1);
    m_AtoB(2, 3) = t(2);

    return true;
}

bool MeTransform::rigidTransform3D(std::vector<MePoint3D> &A, std::vector<MePoint3D> &B, Eigen::Matrix3d &R_AtoB, Eigen::Vector3d &T_AtoB)
{
    assert(A.size() == B.size());

    return rigidTransform3D(A.data(), B.data(), (uint)A.size(), R_AtoB, T_AtoB);
}

bool MeTransform::rigidTransform3D(MePoint3D A[], MePoint3D B[], uint size, Eigen::Matrix3d &R_AtoB, Eigen::Vector3d &T_AtoB)
{
    if (size < 3) {
        std::cout << "The size of A must be greater/equal 3." << std::endl;
        return false;
    }

    // find mean column wise
    MePoint3D centroid_A;
    MePoint3D centroid_B;
    centroid_A.zero(); centroid_B.zero();
    for (unsigned int i=0; i< size; i++) {
        centroid_A += A[i];
        centroid_B += B[i];
    }
    centroid_A /= size;
    centroid_B /= size;

    // subtract mean
    Eigen::MatrixXd Am(3, size);
    Eigen::MatrixXd Bm(3, size);
    for (unsigned int i=0; i< size; i++) {
        Am(0, i) = A[i](0) - centroid_A(0);
        Am(1, i) = A[i](1) - centroid_A(1);
        Am(2, i) = A[i](2) - centroid_A(2);

        Bm(0, i) = B[i](0) - centroid_B(0);
        Bm(1, i) = B[i](1) - centroid_B(1);
        Bm(2, i) = B[i](2) - centroid_B(2);
    }

    // calculate covariance matrix
    Eigen::MatrixXd H = Am * Bm.transpose();

    // std::cout << "H= \n" << H <<std:: endl;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::MatrixXd V = svd.matrixV(), U = svd.matrixU();

    Eigen::MatrixXd R = V*U.transpose();

    // special reflection case
    if (R.determinant() < 0) {
        //std::cout << "det(R) < R, reflection detected!, correcting for it ..." << std::endl;
        V(0, 2) *= -1;
        V(1, 2) *= -1;
        V(2, 2) *= -1;
        R = V*U.transpose();
    }


    Eigen::Vector3d c_A(centroid_A.v);
    Eigen::Vector3d c_B(centroid_B.v);
    R_AtoB = R;
    T_AtoB = c_B - R*c_A;

    return true;
}

bool MeTransform::rigidTransform3D(std::vector<Eigen::Vector3d> &A, std::vector<Eigen::Vector3d> &B, Matrix4d &m_AtoB)
{
    assert(A.size() == B.size());
    return rigidTransform3D(A.data(), B.data(), (uint)A.size(), m_AtoB);
}

bool MeTransform::rigidTransform3D(Eigen::Vector3d A[], Eigen::Vector3d B[], uint size, Matrix4d &m_AtoB)
{
    if (size < 3) {
        std::cout << "The size of A must be greater/equal 3." << std::endl;
        return false;
    }

    // find mean column wise
    Eigen::Vector3d centroid_A = {0, 0, 0};
    Eigen::Vector3d centroid_B = {0, 0, 0};
    for (unsigned int i=0; i< size; i++) {
        centroid_A += A[i];
        centroid_B += B[i];
    }
    centroid_A /= size;
    centroid_B /= size;

    // subtract mean
    Eigen::MatrixXd Am(3, size);
    Eigen::MatrixXd Bm(3, size);
    for (unsigned int i=0; i< size; i++) {
        Am(0, i) = A[i](0) - centroid_A(0);
        Am(1, i) = A[i](1) - centroid_A(1);
        Am(2, i) = A[i](2) - centroid_A(2);

        Bm(0, i) = B[i](0) - centroid_B(0);
        Bm(1, i) = B[i](1) - centroid_B(1);
        Bm(2, i) = B[i](2) - centroid_B(2);
    }

    // calculate covariance matrix
    Eigen::MatrixXd H = Am * Bm.transpose();

    // std::cout << "H= \n" << H <<std:: endl;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::MatrixXd V = svd.matrixV(), U = svd.matrixU();

    Eigen::MatrixXd R = V*U.transpose();

    // special reflection case
    if (R.determinant() < 0) {
        //std::cout << "det(R) < R, reflection detected!, correcting for it ..." << std::endl;
        V(0, 2) *= -1;
        V(1, 2) *= -1;
        V(2, 2) *= -1;
        R = V*U.transpose();
    }

    m_AtoB.identity();
    for (unsigned i=0; i<3;i++) {
        for (unsigned j=0; j<3;j++) {
            m_AtoB(i,j) = R(i,j);
        }
    }

    Eigen::Vector3d t;
    t =  centroid_B - R*centroid_A;
    m_AtoB(0, 3) = t(0);
    m_AtoB(1, 3) = t(1);
    m_AtoB(2, 3) = t(2);

    return true;
}

bool MeTransform::rigidTransform3D(std::vector<cv::Point3d> &A, std::vector<cv::Point3d> &B, Eigen::Matrix3d &R_AtoB, Eigen::Vector3d &T_AtoB)
{
    assert(A.size() == B.size());

    return rigidTransform3D(A.data(), B.data(), uint(A.size()), R_AtoB, T_AtoB);
}

bool MeTransform::rigidTransform3D(std::vector<cv::Point3d> &A, std::vector<cv::Point3d> &B, Matrix4d &m_AtoB)
{
    if (A.size() != B.size()) {
        return false;
    }
    uint size = A.size();
    if (size < 3) {
        std::cout << "The size of A must be greater/equal 3." << std::endl;
        return false;
    }

    // find mean column wise
    cv::Point3d centroid_A(0, 0, 0);
    cv::Point3d centroid_B(0, 0, 0);
    for (unsigned int i=0; i< size; i++) {
        centroid_A += A[i];
        centroid_B += B[i];
    }
    centroid_A /= double(size);
    centroid_B /= double(size);

    // subtract mean
    Eigen::MatrixXd Am(3, size);
    Eigen::MatrixXd Bm(3, size);
    for (unsigned int i=0; i< size; i++) {
        Am(0, i) = A[i].x - centroid_A.x;
        Am(1, i) = A[i].y - centroid_A.y;
        Am(2, i) = A[i].z - centroid_A.z;

        Bm(0, i) = B[i].x - centroid_B.x;
        Bm(1, i) = B[i].y - centroid_B.y;
        Bm(2, i) = B[i].z - centroid_B.z;
    }

    // calculate covariance matrix
    Eigen::MatrixXd H = Am * Bm.transpose();

    // std::cout << "H= \n" << H <<std:: endl;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::MatrixXd V = svd.matrixV(), U = svd.matrixU();

    Eigen::MatrixXd R = V*U.transpose();

    // special reflection case
    if (R.determinant() < 0) {
        // std::cout << "det(R) < R, reflection detected!, correcting for it ..." << std::endl;
        V(0, 2) *= -1;
        V(1, 2) *= -1;
        V(2, 2) *= -1;
        R = V*U.transpose();
    }

    double cAv[] = {centroid_A.x, centroid_A.y, centroid_A.z};
    double cBv[] = {centroid_B.x, centroid_B.y, centroid_B.z};
    Eigen::Vector3d c_A(cAv);
    Eigen::Vector3d c_B(cBv);

    m_AtoB.identity();
    for (unsigned i=0; i<3;i++) {
        for (unsigned j=0; j<3;j++) {
            m_AtoB(i,j) = R(i,j);
        }
    }

    Eigen::Vector3d t;
    t =  c_B - R*c_A;
    m_AtoB(0, 3) = t(0);
    m_AtoB(1, 3) = t(1);
    m_AtoB(2, 3) = t(2);

    return true;
}

bool MeTransform::rigidTransform3D(cv::Point3d A[], cv::Point3d B[], uint size, Eigen::Matrix3d &R_AtoB, Eigen::Vector3d &T_AtoB)
{
    if (size < 3) {
        std::cout << "The size of A must be greater/equal 3." << std::endl;
        return false;
    }

    // find mean column wise
    cv::Point3d centroid_A(0, 0, 0);
    cv::Point3d centroid_B(0, 0, 0);
    for (unsigned int i=0; i< size; i++) {
        centroid_A += A[i];
        centroid_B += B[i];
    }
    centroid_A /= double(size);
    centroid_B /= double(size);

    // subtract mean
    Eigen::MatrixXd Am(3, size);
    Eigen::MatrixXd Bm(3, size);
    for (unsigned int i=0; i< size; i++) {
        Am(0, i) = A[i].x - centroid_A.x;
        Am(1, i) = A[i].y - centroid_A.y;
        Am(2, i) = A[i].z - centroid_A.z;

        Bm(0, i) = B[i].x - centroid_B.x;
        Bm(1, i) = B[i].y - centroid_B.y;
        Bm(2, i) = B[i].z - centroid_B.z;
    }

    // calculate covariance matrix
    Eigen::MatrixXd H = Am * Bm.transpose();

    // std::cout << "H= \n" << H <<std:: endl;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::MatrixXd V = svd.matrixV(), U = svd.matrixU();

    Eigen::MatrixXd R = V*U.transpose();

    // special reflection case
    if (R.determinant() < 0) {
        // std::cout << "det(R) < R, reflection detected!, correcting for it ..." << std::endl;
        V(0, 2) *= -1;
        V(1, 2) *= -1;
        V(2, 2) *= -1;
        R = V*U.transpose();
    }

    double cAv[] = {centroid_A.x, centroid_A.y, centroid_A.z};
    double cBv[] = {centroid_B.x, centroid_B.y, centroid_B.z};
    Eigen::Vector3d c_A(cAv);
    Eigen::Vector3d c_B(cBv);
    R_AtoB = R;
    T_AtoB = c_B - R*c_A;

    return true;
}

void MeTransform::coordBtoARotationMatrix(double p_A[], double px_A[], double py_A[], double pz_A[],
         double dx_B, double dy_B, double dz_B, Matrix4d &rotMatrix)
{
    // rotMatrix.identity();


    double vx[3], vy[3], vz[3];
    MeMath::subtract(px_A, p_A, vx);
    MeMath::subtract(py_A, p_A, vy);
    MeMath::subtract(pz_A, p_A, vz);

    double normX, normY, normZ;
    normX = MeMath::norm(vx); if (dx_B < 0) normX = -normX;
    normY = MeMath::norm(vy); if (dy_B < 0) normY = -normY;
    normZ = MeMath::norm(vz); if (dz_B < 0) normZ = -normZ;

    // 计算旋转变量
    rotMatrix(0,0) = (vx[0])/normX;
    rotMatrix(1,0) = (vx[1])/normX;
    rotMatrix(2,0) = (vx[2])/normX;

    rotMatrix(0,1) = (vy[0])/normY;
    rotMatrix(1,1) = (vy[1])/normY;
    rotMatrix(2,1) = (vy[2])/normY;

    rotMatrix(0,2) = (vz[0])/normZ;
    rotMatrix(1,2) = (vz[1])/normZ;
    rotMatrix(2,2) = (vz[2])/normZ;
}

void MeTransform::calP_J6WithRotMatrix(Matrix4d m_J6toB[], Matrix4d &mRot_AtoB, double p_A[3][3], double outP_J6[])
{
    Matrix4d deltaM_J6toB[2];
    deltaM_J6toB[0] = m_J6toB[1] - m_J6toB[0];
    deltaM_J6toB[1] = m_J6toB[2] - m_J6toB[0];

    double deltaP_A[2][3];
    MeMath::subtract(p_A[1], p_A[0], deltaP_A[0]);
    MeMath::subtract(p_A[2], p_A[0], deltaP_A[1]);
    mRot_AtoB.multiplyPoint3D(deltaP_A[0], deltaP_A[0]);
    mRot_AtoB.multiplyPoint3D(deltaP_A[1], deltaP_A[1]);

    Matrix3d m[3], lower[3], upper[3];
    double b[3][3];
    for (int i=0; i<2;i++) {
        b[i][0] = deltaP_A[i][0] - deltaM_J6toB[i].get(0, 3);
        b[i][1] = deltaP_A[i][1] - deltaM_J6toB[i].get(1, 3);
        b[i][2] = deltaP_A[i][2] - deltaM_J6toB[i].get(2, 3);
        for ( unsigned row = 0; row < 3; ++row )
            for ( unsigned column = 0; column < 3; ++column )
                m[i][ row ][ column ] = deltaM_J6toB[i].get(row, column);
        m[i].LU_Decomposition(upper[i], lower[i]);
        std::cout << "upper:\n" << upper[i] << std::endl;
        std::cout << "lower:\n" << lower[i] << std::endl;
        std::cout << "b: " << b[i][0] << ", " << b[i][1] << ", " << b[i][2] << std::endl;
    }

    // 合并两个矩阵
    for (unsigned i=0; i<3;i++) {
        // upper 是否为0
        if( MeMath::isZero(upper[0].get(i,i)) || MeMath::isZero(lower[0].get(i,i)) ) {
            m[2][i][0] = m[1][i][0];
            m[2][i][1] = m[1][i][1];
            m[2][i][2] = m[1][i][2];
            b[2][i]    = b[1][i];
        } else {
            m[2][i][0] = m[0][i][0];
            m[2][i][1] = m[0][i][1];
            m[2][i][2] = m[0][i][2];
            b[2][i]    = b[0][i];
        }
    }

    m[2].LU_Decomposition(upper[2], lower[2]);
    std::cout << "upper2:\n" << upper[2] << std::endl;
    std::cout << "lower2:\n" << lower[2] << std::endl;

    double Y[3];
    Matrix3d invL = lower[2].getInverse();
    invL.multiplyPoint3D(b[2], Y);

    Matrix3d invU = upper[2].getInverse();
    invU.multiplyPoint3D(Y, outP_J6);
}

Matrix4d MeTransform::coordTransAtoB(double p_A[], double px_A[], double py_A[], double pz_A[], double dx_B, double dy_B, double dz_B, double angleX, double angleY, double pRx_A[], double pRy_A[], double centerRotate_B[])
{
    Matrix4d m;
    m.identity();

    // 计算旋转变量
    m(0,0) = (px_A[0]-p_A[0])/dx_B;
    m(1,0) = (px_A[1]-p_A[1])/dx_B;
    m(2,0) = (px_A[2]-p_A[2])/dx_B;

    m(0,1) = (py_A[0]-p_A[0])/dy_B;
    m(1,1) = (py_A[1]-p_A[1])/dy_B;
    m(2,1) = (py_A[2]-p_A[2])/dy_B;

    m(0,2) = (pz_A[0]-p_A[0])/dz_B;
    m(1,2) = (pz_A[1]-p_A[1])/dz_B;
    m(2,2) = (pz_A[2]-p_A[2])/dz_B;


    // cout << "m=\n" << m << endl;
    double pB[3];

    // 转成B坐标系表示的点
    calPBWithPA(p_A, pRx_A, pRy_A, angleX, angleY, m, centerRotate_B, pB);

    // 计算仅仅旋转后的PAr
    double pAr[3];
    m.multiplyPoint3D(pB, pAr);

    m(0,3) = p_A[0] - pAr[0];
    m(1,3) = p_A[1] - pAr[1];
    m(2,3) = p_A[2] - pAr[2];

    return m;
}

void MeTransform::calPBWithPA(double p_A[], double px_A[], double py_A[],
                  double angleX, double angleY, Matrix4d &m_BtoA,
                  double centerRotate_B[], double outP_B[])
{
    Matrix4d mRotX = MeTransform::rotateX(angleX);
    Matrix4d mRotY =  MeTransform::rotateY(angleY);
    Matrix4d mIdentiy;
    mIdentiy.identity();

    // 先计算Y
    Matrix4d m = m_BtoA * (mIdentiy - mRotY);
    double dx = p_A[0] - py_A[0];
    double dy = p_A[1] - py_A[1];
    double dz = p_A[2] - py_A[2];

    // x, z
    double d = (m(0,0)*m(2,2)-m(0,2)*m(2,0));
    outP_B[0] = (m(2,2)*dx- m(0,2)*dz)/d;
    outP_B[2] = (m(0,0)*dz- m(0,2)*dx)/d;

    m = m_BtoA * (mIdentiy - mRotX);
    dx = p_A[0] - px_A[0];
    dy = p_A[1] - px_A[1];
    dz = p_A[2] - px_A[2];
    // y
    outP_B[1] = (m(2,2)*dy- m(1,2)*dz)/(m(1,1)*m(2,2)-m(1,2)*m(2,1));

    // 前面计算的是以centerRotate_B旋转的p点坐标(用B表示)，实际是C坐标系表是的点
    // 转成B坐标系表示的点
    MeMath::add(centerRotate_B, outP_B, outP_B);

    //cout << "delta P{A}: " << dx << "," << dy << "," << dz << endl;
    //cout << "===m=\n" << m << endl;
    // cout << "== outP_B:" << outP_B[0] <<"," << outP_B[1] << "," << outP_B[2] << endl;
}

Eigen::Vector3d MeTransform::calibTcpPoint(std::vector<Eigen::Matrix3d> &R, std::vector<Eigen::Vector3d> &t)
{
    // $ p_{bt} = R_{be} * p_{et} + t_{be} $

    assert(R.size() == t.size());
    int nGroup = R.size();
    assert(nGroup >=4);

    Eigen::MatrixXd A((nGroup - 1)*3, 3);
    Eigen::VectorXd b((nGroup - 1)*3);
    uint i = 0,j,k;
    for (i = 0; i< R.size()-1; i++) {
        for (j = 0; j < 3; j++) {
            for(k = 0; k<3; k++) {
                A(i*3+j, k) = R[i](j,k) - R[i+1](j,k);

            }
            b[i*3+j] = t[i+1][j] - t[i][j];
        }
    }

    //cout << "A:\n" << A << endl;
    //cout << "b:\n" << b << endl;

    //Eigen::Vector3d x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    Eigen::Vector3d x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

    // cout << "result: \n" << x << endl;
    return x;
}

